﻿////////////////////////////////////////////////////////////////////////////
//
// Copyright 2013-2019; 2023 Intel Corporation
// SPDX-License-Identifier: Apache-2.0
//
//
// Bandpassfilter.cs
//
// Equirriple bandpass filter 2-40Hz to filter EEG data
//
////////////////////////////////////////////////////////////////////////////

using Accord.Math;
using System;
using System.Collections.Generic;

namespace ACAT.Extensions.BCI.Actuators.EEG.EEGProcessing.Filters
{
    /// <summary>
    /// Equirriple bandpass filter with:
    ///
    //Fstop1 = 1;      % First Stopband Frequency
    //Fpass1 = 5;      % First Passband Frequency
    //Fpass2 = 35;     % Second Passband Frequency
    //Fstop2 = 40;     % Second Stopband Frequency
    //Dstop1 = 0.001;  % First Stopband Attenuation
    //Dpass = 0.007;  % Passband Ripple
    //Dstop2 = 0.001;  % Second Stopband Attenuation
    //dens = 20;     % Density Factor
    // Ncoefs=153
    // FilterDelay=78

    /// </summary>
    [Serializable]
    public class BandpassFilter
    {
        /// <summary>
        /// Group delay of the filter
        /// </summary>
        private readonly int groupDelay;

        private readonly int nCoeffs;

        /// <summary>
        /// Coeficients of the filters
        /// </summary>
        private readonly double[] b = null;

        private readonly double[] a = null;

        public BandpassFilter(int sampleRate)
        {
            switch (sampleRate)
            {
                case 125:
                    b = new double[] { -0.00197589067846126, -0.00251591005701485, -0.000686449722035674, 0.000984540265601032, 8.98070000427201e-06, -0.000643168258331260, 0.00145028961988512, 0.00232041625243436, 0.000566329230969635, 0.00178415357135950, 0.00505552295400874, 0.00341463427171445, 0.00171447459305044, 0.00644872019422238, 0.00742837398369570, 0.00186255723900787, 0.00464267825633150, 0.0104576861944426, 0.00308388291148590, -0.000908838287575232, 0.00907509671817434, 0.00532714976902270, -0.00857605772646215, 0.000151557460314324, 0.00661098770445157, -0.0148799500665480, -0.0169777260187762, 0.00292094208377287, -0.0161023260497770, -0.0388558640677809, -0.0106536591626340, -0.0104066292961136, -0.0576249980481401, -0.0382849380928292, 0.000774339103799522, -0.0614834627868744, -0.0842459582836306, 0.0130242290735603, -0.0276151466277104, -0.176018540190786, 0.0209699379219639, 0.458930211513167, 0.458930211513167, 0.0209699379219639, -0.176018540190786, -0.0276151466277104, 0.0130242290735603, -0.0842459582836306, -0.0614834627868744, 0.000774339103799522, -0.0382849380928292, -0.0576249980481401, -0.0104066292961136, -0.0106536591626340, -0.0388558640677809, -0.0161023260497770, 0.00292094208377287, -0.0169777260187762, -0.0148799500665480, 0.00661098770445157, 0.000151557460314324, -0.00857605772646215, 0.00532714976902270, 0.00907509671817434, -0.000908838287575232, 0.00308388291148590, 0.0104576861944426, 0.00464267825633150, 0.00186255723900787, 0.00742837398369570, 0.00644872019422238, 0.00171447459305044, 0.00341463427171445, 0.00505552295400874, 0.00178415357135950, 0.000566329230969635, 0.00232041625243436, 0.00145028961988512, -0.000643168258331260, 8.98070000427201e-06, 0.000984540265601032, -0.000686449722035674, -0.00251591005701485, -0.00197589067846126 };
                    a = new double[] { 1 };
                    groupDelay = 41;
                    nCoeffs = b.Length;
                    break;

                case 250:
                    b = new double[] { -0.00008778031024074279, -0.00189352391427887, -0.00354614862967617, -0.00482644914994822, -0.00461049309823788, -0.00292541015225790, -0.000999016197721182, -0.000458439212884342, -0.00196731602169841, -0.00448978288166562, -0.00599480913680623, -0.00517769037381800, -0.00272823678886284, -0.000888205487140518, -0.00150775253404725, -0.00427783950544068, -0.00684333926300974, -0.00687612467373002, -0.00430074888329805, -0.00153804385919643, -0.00135619533958771, -0.00421422719762474, -0.00764695311776498, -0.00846006972407959, -0.00580724466931299, -0.00219134871036170, -0.00123890874960981, -0.00419666875380569, -0.00852310710355057, -0.0101359977603706, -0.00736260349594594, -0.00278672899224414, -0.000985072393420895, -0.00409635772074326, -0.00949106280881485, -0.0120439609699949, -0.00909248627484281, -0.00331123254949911, -0.000442350266661742, -0.00374995305613202, -0.0105204219915174, -0.0143091656986992, -0.0111468624554879, -0.00377014576885177, 0.000570284349154347, -0.00293454841280232, -0.0115525254185011, -0.0170978209291922, -0.0137437647742755, -0.00417136565242170, 0.00233797534660630, -0.00129654267816600, -0.0125169122993329, -0.0207379205547141, -0.0172980003280536, -0.00451632738357270, 0.00544209474030187, 0.00186886729904535, -0.0133447313597268, -0.0260697617732556, -0.0228444427733559, -0.00479694624403622, 0.0114260438680806, 0.00846170944285514, -0.0139780529312683, -0.0358964923345268, -0.0339669953403191, -0.00499810249861452, 0.0264298832225855, 0.0267666026759814, -0.0143747151379005, -0.0667787947789595, -0.0759992741088555, -0.00510376960621198, 0.130200216285966, 0.263418579541359, 0.318823652378022, 0.263418579541359, 0.130200216285966, -0.00510376960621198, -0.0759992741088555, -0.0667787947789595, -0.0143747151379005, 0.0267666026759814, 0.0264298832225855, -0.00499810249861452, -0.0339669953403191, -0.0358964923345268, -0.0139780529312683, 0.00846170944285514, 0.0114260438680806, -0.00479694624403622, -0.0228444427733559, -0.0260697617732556, -0.0133447313597268, 0.00186886729904535, 0.00544209474030187, -0.00451632738357270, -0.0172980003280536, -0.0207379205547141, -0.0125169122993329, -0.00129654267816600, 0.00233797534660630, -0.00417136565242170, -0.0137437647742755, -0.0170978209291922, -0.0115525254185011, -0.00293454841280232, 0.000570284349154347, -0.00377014576885177, -0.0111468624554879, -0.0143091656986992, -0.0105204219915174, -0.00374995305613202, -0.000442350266661742, -0.00331123254949911, -0.00909248627484281, -0.0120439609699949, -0.00949106280881485, -0.00409635772074326, -0.000985072393420895, -0.00278672899224414, -0.00736260349594594, -0.0101359977603706, -0.00852310710355057, -0.00419666875380569, -0.00123890874960981, -0.00219134871036170, -0.00580724466931299, -0.00846006972407959, -0.00764695311776498, -0.00421422719762474, -0.00135619533958771, -0.00153804385919643, -0.00430074888329805, -0.00687612467373002, -0.00684333926300974, -0.00427783950544068, -0.00150775253404725, -0.000888205487140518, -0.00272823678886284, -0.00517769037381800, -0.00599480913680623, -0.00448978288166562, -0.00196731602169841, -0.000458439212884342, -0.000999016197721182, -0.00292541015225790, -0.00461049309823788, -0.00482644914994822, -0.00354614862967617, -0.00189352391427887, -0.00008778031024074279 };
                    a = new double[1] { 1 }; //Denum
                    groupDelay = 76;
                    nCoeffs = b.Length;
                    break;
            }
        }

        /// <summary>
        /// Get group delay of the filter
        /// </summary>
        /// <returns></returns>
        public int GetGroupDelay()
        {
            return groupDelay;
        }

        /// <summary>
        /// Filters samples and compensates from delay in triggersignal
        /// </summary>
        /// <param name="data"> in [numChannels, numSamples] </param>
        /// <returns></returns>
        public void FilterData(double[,] unfilteredData, int[] triggerSignal, out double[,] filteredData, out int[] delayedTriggerSignal)
        {
            filteredData = FilterData(unfilteredData);

            // Delay trigger signal to adjust with delay that filter introduces to data
            List<int> afterFilter = new();
            for (int i = 0; i < groupDelay; i++)
                afterFilter.Add(0);
            for (int sampleIdx = 0; sampleIdx < triggerSignal.Length - groupDelay; sampleIdx++)
                afterFilter.Add(triggerSignal[sampleIdx]);

            delayedTriggerSignal = afterFilter.ToArray();
        }

        /// <summary>
        /// Filters samples in
        /// </summary>
        /// <param name="data"> in [numChannels, numSamples] </param>
        /// <returns></returns>
        public double[,] FilterData(double[,] unfilteredData)
        {
            int numChannels = unfilteredData.GetLength(0);
            int numSamples = unfilteredData.GetLength(1);

            double[,] filteredData = new double[numChannels, numSamples];

            for (int channelIdx = 0; channelIdx < numChannels; channelIdx++)
            {
                //y[n]=b0x[n]+b1x[n-1]+....bmx[n-M]
                double[] y = new double[numSamples];
                for (int yi = 0; yi < numSamples; yi++)
                {
                    // Apply filter
                    double tmp = 0;
                    for (int bi = nCoeffs - 1; bi >= 0; bi--)
                    {
                        if (yi - bi < 0) continue;

                        tmp += b[bi] * unfilteredData[channelIdx, yi - bi];
                    }
                    y[yi] = tmp;
                }
                filteredData.SetRow(channelIdx, y);
            }
            return filteredData;
        }
    }
}